#include <__cross_studio_io.h>
#include <msp430x14x.h>
#include <stdlib.h>
#include "bcd.h" // declare bdc_t could be just unsigned long or alike

void main(void){ // main neither has arguments nor returns anything
// Test code here
  int k = 0;
  bcd32_t elements[] = {0x00000001, 0x00000008, 0x00000017, 0x00000034, 0x0000065, 0, 0};
  //debug_printf("bcdadd %#1lx",two);
  babbage(3, 7, elements);
  debug_exit(0);
}

void babbage(unsigned int PolyOrder, unsigned int NumItems, bcd32_t* Elements)
{
  int i, j, head, tail;
  bcd32_t a,b;
  bcd32_t* items = (bcd32_t *)malloc((PolyOrder + 1) * sizeof(bcd32_t) * (NumItems));

  for(i = 0; i < NumItems; i++)
    if(Elements[i] != 0)
      tail = i;

  if(tail < PolyOrder)  //disable this statement to disable validation
  {
    debug_printf("Not enough items");
    debug_exit(0);
  }

  for(i = 0; i < NumItems; i++)
    items[i] = Elements[i];

  for(i = NumItems; i < NumItems * (PolyOrder + 1); i++)
  {
    if(items[i - NumItems + 1] != 0 && items[i - NumItems] != 0)
    {
      a = items[i - NumItems + 1];
      b = items[i - NumItems] ^ 0x10000000;
      debug_printf("a: 0x%08lx ",a);
      debug_printf("b_negative: 0x%08lx ",b);
      debug_printf("i: %d ",i);
      bcdadd(&a, &b);
      items[i] = b;
      debug_printf("sub: 0x%08lx ",items[i]);
    }
  }

  for(i = tail + 1; i < NumItems; i++)
  {
    for(j = PolyOrder; j >= 0; j--)
    {
      if(j == PolyOrder)
        items[j * NumItems - j + i] = items[j * NumItems - j + i - 1];
      else
      {
        a = items[j * NumItems - j + i - 1];
        b = items[j * NumItems - j + i - 1 + NumItems];
        bcdadd(&a, &b);
        debug_printf("b: 0x%08lx ",b);
        //items[j * NumItems - j + i] = items[j * NumItems - j + i - 1] + items[j * NumItems - j + i - 1 + NumItems]; 
        items[j * NumItems - j + i] = b;
      }
    }
  }

  for(i = tail; i < NumItems; i++ )
    Elements[i] = items[i];

  for(i = 0; i < NumItems; i++ )
    debug_printf("0x%08lx|", Elements[i]);
}

/*void babbage(unsigned int PolyOrder, unsigned int NumItems, bcd32_t* Elements)
{
  int i=0;
  int j=0;
  bcd32_t placeHolder;
  bcd32_t* differences = (bcd32_t*) malloc ((PolyOrder)*(sizeof(bcd32_t*)));
  
  debug_printf("%X", &PolyOrder);
  debug_printf("%X", &Elements);
  for(i=0; i<PolyOrder; i++)
  {
     placeHolder = Elements[i];
     Elements[i] *= -1;
  debug_printf("%X", &PolyOrder);
     bcdadd(&Elements[i+1],&Elements[i]);
     differences[i] = Elements[i];
     Elements[i] = placeHolder;
  }
  
  for(j=1;j<PolyOrder;j++)
  {
    for(i=j; i<PolyOrder; i++)
    {
      differences[i-1] *= -1;
      bcdadd(&differences[i-1],&differences[i]);
      differences[i-1] *= -1;
    }
  }
  
  for(i=0; i<NumItems; i++)
  {
    Elements[i+1] = differences[0];
    bcdadd(&Elements[i],&Elements[i+1]);
    debug_printf(" %#1lx \n",Elements[i]);
    for(j=1; j<PolyOrder; j++)
    {
      bcdadd(&differences[j],&differences[j-1]);
      //differences[j-1] += differences[j];
    }
  }
}*/
// babbage uses finite differences to evaluate the function value x=f(p)
// list parameters here as bcd numbers
/*void babbage(unsigned int PolyOrder, unsigned int NumItems, bcd32_t* Elements) 
{
  int i = 0;
  int j = 0;
  int k = 0;
  int p = 0;
  bcd32_t nextElement=0x00000000; 
  //bcd32_t elementsArray [NumItems][PolyOrder + 2];
  bcd32_t** elementsArray = (bcd32_t**)malloc(NumItems*(sizeof (bcd32_t*)));
  for (p=0; p<NumItems; p++)
  {
     elementsArray[p] = (bcd32_t*)malloc((PolyOrder+2)*sizeof(bcd32_t));
  }
  //elementsArray = new bcd32_t [NumItems][PolyOrder + 2];

  for(i=0; i<=PolyOrder; i++)
  {
    elementsArray[0][i] = Elements[i];
  }
  for(j=1; j<=PolyOrder; j++)
  {
    for(i=0; i<=PolyOrder-j; i++)
    {
      elementsArray[j][i] = elementsArray[j-1][i+1] - elementsArray[j-1][i];
    }
  }
  for(i=PolyOrder+1; i<NumItems; i++)
  {
    nextElement = 0x00000000;
    for(j=PolyOrder,k=i-(PolyOrder+1);j>=0,k<i;j--,k++)
    {
     bcdadd(&nextElement, &elementsArray[j][k]);
     nextElement = elementsArray[j][k]; 
    }
    elementsArray[0][i] = nextElement;
  }
  for (i=0; i<NumItems; i++)
  {
    debug_printf("%d",elementsArray[0][i]);
  }
}
// other functions come here, if needed
// other parameters interpreted as other data types*/